#include "pfsm_servo_filter.h"
#include "zuc/nml_intf/zucpos2.h"
#include <math.h>

using namespace axisgroup;

#ifndef MIN
#define MIN(a, b) ((a) < (b) ? (a) : (b))
#endif
#ifndef MAX
#define MAX(a, b) ((a) > (b) ? (a) : (b))
#endif

static double ffsign(double f)
{
    if (fabs(f) < 1e-10)
    {
        return 0.0;
    }
    if (f > 0)
    {
        return 1.0;
    }
    else if (f < 0)
    {
        return -1.0;
    }
    else
    {
        //Technically this should be NAN but that's a useless result for tp purposes
        return 0.0;
    }
}

enum FilterType
{
    NONE = 0,
    LPF_1ST = 1,
    NLF_JOINT = 2,
    MMF_JOINT = 3,
    NLF_CART = 4,
    NLF_MMF_COMB = 5,
};

std::map<ServoFilterIntf::FilterType, std::string> ServoFilterIntf::FILTER_TYPE_NAME = {{ServoFilterIntf::FilterType::NONE, "NONE"},
                                                                                        {ServoFilterIntf::FilterType::LPF_1ST, "LPF_1ST"},
                                                                                        {ServoFilterIntf::FilterType::NLF_JOINT, "NLF_JOINT"},
                                                                                        {ServoFilterIntf::FilterType::MMF_JOINT, "MMF_JOINT"},
                                                                                        {ServoFilterIntf::FilterType::NLF_CART, "NLF_CART"},
                                                                                        {ServoFilterIntf::FilterType::NLF_MMF_COMB, "NLF_MMF_COMB"}};

ServoFilterIntf::ServoFilterIntf()
{
    mmf_filter_ = std::make_shared<MMFFilter>();
    nlf_filter_ = std::make_shared<NLFFilter>();
    lpf_filter_ = std::make_shared<LPF1Filter>();
}

void ServoFilterIntf::do_cart_filt(ZucPose& positions)
{
    double pos[6] = {positions.tran.x, positions.tran.y, positions.tran.z, positions.a, positions.b, positions.c};
    do_cart_filt(pos);
    positions.tran.x = pos[0];
    positions.tran.y = pos[1];
    positions.tran.z = pos[2];
    positions.a = pos[3];
    positions.b = pos[4];
    positions.c = pos[5];
};

void ServoFilterIntf::do_cart_filt(double* positions)
{  // 笛卡尔滤波
    if (NLF_MMF_COMB == servo_filter_type_ && mmf_filter_.get())
    {
        for (int j = 0; j < 6; j++)
        {
            mmf_filter_->push_buffer_data(positions[j], j);  // 无意义
            positions[j] = mmf_filter_->mmf_process_cart(j);
        }
    }
    else if (NLF_CART == servo_filter_type_ && nlf_filter_.get())
    {
        for (int j = 0; j < 6; j++)
        {
            nlf_filter_->nlf_set_target(j, positions[j]);
            nlf_filter_->nlf_c3(j);
            positions[j] = nlf_filter_->nlf_data[j].cur_pos;
        }
    }
}

void ServoFilterIntf::do_joint_filter(double* cmd_val)
{
    if (LPF_1ST == servo_filter_type_ && lpf_filter_.get())
    {
        for (size_t joint_num = 0; joint_num < joint_num_; joint_num++)
        {
            lpf_filter_->lpf_set_target(joint_num, cmd_val[joint_num]);
            lpf_filter_->lpf_process(joint_num);
            cmd_val[joint_num] = lpf_filter_->lpf1st_data[joint_num].curr_pos;
        }
    }
    else if (NLF_JOINT == servo_filter_type_ && nlf_filter_.get())
    {
        for (size_t joint_num = 0; joint_num < joint_num_; joint_num++)
        {
            nlf_filter_->nlf_set_target(joint_num, cmd_val[joint_num]);
            nlf_filter_->nlf_c3(joint_num);
            cmd_val[joint_num] = nlf_filter_->nlf_data[joint_num].cur_pos;
        }
    }
    else if (MMF_JOINT == servo_filter_type_ && mmf_filter_.get())
    {
        for (size_t joint_num = 0; joint_num < joint_num_; joint_num++)
        {
            //每个周期更新一个数据
            mmf_filter_->push_buffer_data(cmd_val[joint_num], joint_num);
            //根据缓存的数据计算目标速度，加速度，jerk
            cmd_val[joint_num] = mmf_filter_->mmf_process(joint_num);
        }
    }
}

std::string ServoFilterIntf::get_filter_name() { return ServoFilterIntf::FILTER_TYPE_NAME[servo_filter_type_]; }

void ServoFilterIntf::set_lpf_para(double lpf_cutoff_freq)
{
    for (int i = 0; i < 7; i++) { lpf_filter_->lpf1st_data[i].cutoff_freq = lpf_cutoff_freq; }
}

void ServoFilterIntf::set_nlf_para(double nlf_vp, double nlf_ap, double nlf_jp, double nlf_vr, double nlf_ar, double nlf_jr)
{
    nlf_filter_->nlf_vp_ = nlf_vp;
    nlf_filter_->nlf_ap_ = nlf_ap;
    nlf_filter_->nlf_jp_ = nlf_jp;
    nlf_filter_->nlf_vr_ = nlf_vr;
    nlf_filter_->nlf_ar_ = nlf_ar;
    nlf_filter_->nlf_jr_ = nlf_jr;
}

void ServoFilterIntf::set_mmf_para(int mmf_buf_size, double mmf_ka, double mmf_kv, double mmf_kp)
{
    mmf_filter_->mmf_buf_size_ = mmf_buf_size;
    mmf_filter_->mmf_ka_ = mmf_ka;
    mmf_filter_->mmf_kv_ = mmf_kv;
    mmf_filter_->mmf_kp_ = mmf_kp;
}

void ServoFilterIntf::init_servo_filter(int joint_num, AxisGroupPose* cmd_pose, const double* vel_limit, const double* acc_limit, double time)
{
    joint_num_ = joint_num;
    // 根据滤波器类型初始化相应的滤波器
    switch (servo_filter_type_)
    {
    case LPF_1ST: {
        // 低通滤波器一律对最终输出关节角度进行滤波
        for (size_t i = 0; i < joint_num_; i++) { lpf_filter_->lpf_init(i, cmd_pose->jpos[i], time); }
        break;
    }

    case NLF_JOINT: {
        for (size_t i = 0; i < joint_num_; i++)
        {
            double tmp_nlf_vr = nlf_filter_->nlf_vr_ < vel_limit[i] ? nlf_filter_->nlf_vr_ : vel_limit[i];
            double tmp_nlf_ar = nlf_filter_->nlf_ar_ < acc_limit[i] ? nlf_filter_->nlf_ar_ : acc_limit[i];
            nlf_filter_->nlf_init(i, cmd_pose->jpos[i]);
            nlf_filter_->nlf_set_bounds(i, tmp_nlf_vr, tmp_nlf_ar, nlf_filter_->nlf_jr_, time);
        }
        break;
    }
    case MMF_JOINT: {
        mmf_filter_->mmf_init(mmf_filter_->mmf_buf_size_, cmd_pose->jpos.val);
        break;
    }
    case NLF_MMF_COMB: {
        mmf_filter_->mmf_init(mmf_filter_->mmf_buf_size_, cmd_pose->pose_array());
        break;
    }
    case NLF_CART: {
        for (int axis_num = 0; axis_num < 6; axis_num++)
        {
            nlf_filter_->nlf_init(axis_num, cmd_pose->pose_array()[axis_num]);
            if (axis_num < 3)
            {
                nlf_filter_->nlf_set_bounds(axis_num, nlf_filter_->nlf_vp_, nlf_filter_->nlf_ap_, nlf_filter_->nlf_jp_, time);
            }
            else
            {
                nlf_filter_->nlf_set_bounds(axis_num, nlf_filter_->nlf_vr_, nlf_filter_->nlf_ar_, nlf_filter_->nlf_jr_, time);
            }
        }
        break;
    }
    default:
        break;
    }
}

int NLFFilter::nlf_init(int joint, double pos)
{
    NLFInfo* q = &(this->nlf_data[joint]);

    q->cur_acc = 0.0;
    q->cur_pos = pos;
    q->cur_vel = 0.0;
    q->cur_jerk = 0.0;
    q->max_jerk = 0.0;
    q->max_acc = 0.0;
    q->max_vel = 0.0;
    q->Ts = 0.0;
    q->target_pos = pos;
    return 0;
}

int NLFFilter::nlf_set_target(int joint, double pos)
{
    NLFInfo* q = &(this->nlf_data[joint]);

    q->target_pos = pos;
    return 0;
}

int NLFFilter::nlf_set_bounds(int joint, double vel, double acc, double jerk, double Ts)
{
    NLFInfo* q = &(this->nlf_data[joint]);

    q->max_vel = vel;
    q->max_acc = acc;
    q->max_jerk = jerk;
    q->Ts = Ts;
    // U = fabs(q->max_jerk);
    return 0;
}

double NLFFilter::u_a(double a, double dde_k) { return -U * ffsign(dde_k - a); }

double NLFFilter::delta_v(double v, double dde_k, double de_k) { return dde_k * fabs(dde_k) + 2 * (de_k - v); }

double NLFFilter::u_cv(double v, double dde_k, double de_k)
{
    return -U * ffsign(delta_v(v, dde_k, de_k) + (1 - fabs(ffsign(delta_v(v, dde_k, de_k)))) * dde_k);
}

double NLFFilter::u_v(double v, double dde_k, double dde_min, double dde_max, double de_k)
{
    return MAX(u_a(dde_min, dde_k), MIN(u_cv(v, dde_k, de_k), u_a(dde_max, dde_k)));
}

int NLFFilter::nlf_c3(int joint)
{
    NLFInfo* q = &(this->nlf_data[joint]);

    double r_k, dr_k, ddr_k;
    double q_k, dq_k, ddq_k;
    //double q_k_1,dq_k_1,ddq_k_1;
    double Ts;
    double e_k, de_k, dde_k;
    double de_min, dde_min, de_max, dde_max;
    double v_min, v_max, a_min, a_max;

    double delta, sigma, v_plus, v_minus;
    double SIGMA;
    double uc, uk;
    double sg;
    U = fabs(q->max_jerk);

    dr_k = 0.0;
    ddr_k = 0.0;
    r_k = q->target_pos;
    Ts = q->Ts;
    // uk = q->cur_jerk;
    q_k = q->cur_pos;
    dq_k = q->cur_vel;
    ddq_k = q->cur_acc;
    v_min = -q->max_vel;
    a_min = -q->max_acc;
    v_max = q->max_vel;
    a_max = q->max_acc;
    //计算rk

    e_k = (q_k - r_k) / U;
    //rtapi_print(" \n q_k = %lf  r_k = %lf e_k = %lf \n",q_k,r_k,e_k);
    //if(pausing)
    //{
    //	rtapi_print(" ----------------------------------\n");
    //	e_k = 0;
    //}
    de_k = (dq_k - dr_k) / U;
    dde_k = (ddq_k - ddr_k) / U;

    de_min = (v_min - dr_k) / U;
    dde_min = (a_min - ddr_k) / U;

    de_max = (v_max - dr_k) / U;
    dde_max = (a_max - ddr_k) / U;
    //rtapi_print(" de_k = %lf  dde_k = %lf de_min = %lf dde_min = %lf  \n",de_k,dde_k,de_min,dde_min);
    delta = de_k + dde_k * fabs(dde_k) / 2;
    sg = ffsign(delta);
    //rtapi_print(" delta  = %lf  sg = %lf \n",delta,sg);
    double temp = pow(dde_k, 2) + 2 * de_k * sg;
    sigma = e_k + de_k * dde_k * sg - pow(dde_k, 3) / 6.0 * (1 - 3.0 * fabs(sg)) + sg / 4.0 * sqrt(2.0 * (pow(temp, 3)));

    v_plus = e_k - dde_max * (pow(dde_k, 2) - 2 * de_k) / 4.0 - pow((pow(dde_k, 2) - 2 * de_k), 2) / 8.0 / dde_max - dde_k * (3 * de_k - pow(dde_k, 2)) / 3.0;
    v_minus = e_k - dde_min * (pow(dde_k, 2) + 2 * de_k) / 4.0 - pow((pow(dde_k, 2) + 2 * de_k), 2) / 8.0 / dde_min + dde_k * (3 * de_k + pow(dde_k, 2)) / 3.0;

    //rtapi_print(" sigma  = %lf	v_plus = %lf v_minus = %lf \n",sigma,v_plus,v_minus);

    if (dde_k <= dde_max && de_k <= pow(dde_k, 2) / 2.0 - pow(dde_max, 2))
    {
        SIGMA = v_plus;
    }
    else if (dde_k >= dde_min && de_k >= (pow(dde_min, 2) - pow(dde_k, 2) / 2.0))
    {
        SIGMA = v_minus;
    }
    else
    {
        SIGMA = sigma;
    }

    uc = -U * ffsign(SIGMA + (1 - fabs(ffsign(SIGMA))) * (delta + (1 - fabs(sg)) * dde_k));
    uk = MAX(u_v(de_min, dde_k, dde_min, dde_max, de_k), MIN(uc, u_v(de_max, dde_k, dde_min, dde_max, de_k)));

    //更新
    q->cur_jerk = uk;

    //rtapi_print(" uc = %f uk  = %lf	 \n",uc,uk);

    q->cur_acc = ddq_k + Ts * uk;
    q->cur_vel = dq_k + Ts / 2.0 * (ddq_k + q->cur_acc);
    q->cur_pos = q_k + Ts / 2.0 * (dq_k + q->cur_vel);

    return 0;
}

int LPF1Filter::lpf_init(int joint, double pos, double fs)
{
    LPF1stInfo* q = &(this->lpf1st_data[joint]);
    q->curr_pos = pos;
    q->tar_pos = pos;
    fs_ = fs;
    return 0;
}

int LPF1Filter::lpf_set_target(int joint, double pos)
{
    LPF1stInfo* q = &(this->lpf1st_data[joint]);
    q->tar_pos = pos;
    return 0;
}

int LPF1Filter::lpf_process(int joint)
{
    LPF1stInfo* q = &(this->lpf1st_data[joint]);
    double cutoff_freq = lpf1st_data[joint].cutoff_freq;
    double coef = 1.0;
    if (cutoff_freq > 0)
    {
        coef = fs_ / (fs_ + 1.0 / (2 * 3.1415 * cutoff_freq));
    }
    q->curr_pos = coef * q->tar_pos + (1 - coef) * q->curr_pos;
    return 0;
}

void MMFFilter::mmf_init(int size, std::vector<double> pos)
{
    mmf_buf_size_ = size;
    filter_buffer_data.clear();
    filter_buffer_data.resize(pos.size());
    for (size_t i = 0; i < pos.size(); i++)
    {
        for (int j = 0; j < size * 2; j++) { filter_buffer_data[i].push_back(pos[i]); };
        target_vel_[i] = 0.0;
        target_acc_[i] = 0.0;
        target_pos_[i] = pos[i];
        target_pos_pre_[i] = target_pos_[i];
    }
}

void MMFFilter::mmf_init(int size, const double* pos)
{
    mmf_buf_size_ = size;
    filter_buffer_data.clear();
    filter_buffer_data.resize(7);
    for (size_t i = 0; i < 7; i++)
    {
        for (int j = 0; j < size * 2; j++) { filter_buffer_data[i].push_back(pos[i]); };
    }
}

void MMFFilter::push_buffer_data(double cmd, int joint_num)
{
    int size = mmf_buf_size_ * 2;
    for (int i = 1; i < size; i++) { filter_buffer_data[joint_num][i - 1] = filter_buffer_data[joint_num][i]; }
    filter_buffer_data[joint_num].back() = cmd;
}

double MMFFilter::mmf_process(int joint_num)
{
    int j = joint_num;
    int M = mmf_buf_size_;
    std::vector<double> test_vel(M * 2);
    std::vector<double> test_acc(M * 2);

    for (int i = 1; i < M * 2; i++) { test_vel[i] = filter_buffer_data[joint_num][i] - filter_buffer_data[joint_num][i - 1]; }
    for (int i = 0; i < M; i++) { test_acc[i] = (test_vel[i + M] - test_vel[i]) / M; }

    double test_jrk = (test_acc[M - 1] - test_acc[1]) / (M - 2);

    double filtered_pos = sum_buffer_data(M - 3, M + 1, j) / 5;
    double filtered_vel = (sum_buffer_data(M, 2 * M - 1, j) - sum_buffer_data(0, M - 1, j)) / M / (M - 1);
    double filtered_acc =
        ((filter_buffer_data[j][(2 * M - 1)] - filter_buffer_data[j][M]) - (filter_buffer_data[j][M - 1] - filter_buffer_data[j][0])) / M / (M - 1);

    target_jrk_[j] = test_jrk + mmf_ka_ * (filtered_acc - target_acc_[j]);
    target_acc_[j] = target_acc_[j] + target_jrk_[j] + mmf_kv_ * (filtered_vel - target_vel_[j]);
    target_vel_[j] = target_vel_[j] + target_acc_[j] + mmf_kp_ * (filtered_pos - target_pos_[j]);
    target_pos_[j] = target_pos_[j] + target_vel_[j];
    return target_pos_[j];
}

double MMFFilter::mmf_process_cart(int axis_num)
{
    int j = axis_num;
    double target_pos_lkahd = target_pos_[j];
    target_pos_[j] = target_pos_[j] + target_vel_[j] + target_acc_[j];
    target_vel_[j] = target_pos_[j] - target_pos_pre_[j];
    target_acc_[j] = mmf_kp_ * (target_pos_lkahd - (target_pos_[j] + target_vel_[j] * mmf_buf_size_));
    target_pos_pre_[j] = target_pos_[j];
    return target_pos_[j];
}

double MMFFilter::sum_buffer_data(int start, int end, int joint_num)
{
    double sum = 0.0;
    for (int i = start; i <= end; i++) { sum += filter_buffer_data[joint_num][i]; }
    return sum;
}
